#include "gns.h"

typedef struct
{
	union {
		struct
		{
			uint8_t Star : 1;		
			uint8_t Term_Num : 5;   
			uint8_t Term_Pos : 5;   
			uint8_t Statement : 4;  
			uint8_t GNGSV_Num : 3;  
			uint8_t GNGSV_Nums : 3;
			uint8_t Received : 6;  
		} F;
	} Flags;
	char Term[13];	 
	char Statement[7];
	uint8_t CRC;	
} GNS_Int_t;

#define CHARISNUM(x) ((x) >= '0' && (x) <= '9')
#define CHARISHEXNUM(x) (((x) >= '0' && (x) <= '9') || ((x) >= 'a' && (x) <= 'f') || ((x) >= 'A' && (x) <= 'F'))
#define CHARTONUM(x) ((x) - '0')
#define CHARHEXTONUM(x) (((x) >= '0' && (x) <= '9') ? ((x) - '0') : (((x) >= 'a' && (x) <= 'z') ? ((x) - 'a' + 10) : (((x) >= 'A' && (x) <= 'Z') ? ((x) - 'A' + 10) : 0)))
#define FROMMEM(x) ((const char *)(x))

#define GNS_ADDTOCRC(ch)          \
	do                            \
	{                             \
		Int.CRC ^= (uint8_t)(ch); \
	} while (0)
#define GNS_ADDTOTERM(ch)                        \
	do                                           \
	{                                            \
		Int.Term[Int.Flags.F.Term_Pos++] = (ch); \
		Int.Term[Int.Flags.F.Term_Pos] = 0;      \
	} while (0); /* Add new element to term object */
#define GNS_START_NEXT_TERM()     \
	do                            \
	{                             \
		Int.Term[0] = 0;          \
		Int.Flags.F.Term_Pos = 0; \
		Int.Flags.F.Term_Num++;   \
	} while (0);

#define GNS_EARTH_RADIUS 6371.0f							   /* 地球版本 */
#define GNS_DEGREES2RADIANS(x) (float)((x)*0.01745329251994f)  /* 度->弧度 */
#define GNS_RADIANS2DEGREES(x) (float)((x)*57.29577951308232f) /* 弧度->度 */
#define GNS_MAX_SATS_IN_VIEW 24								  


#define GNS_UNKNOWN 0
#define GNS_GNGGA 1
#define GNS_GNGSA 2
#define GNS_GNGSV 3
#define GNS_GNRMC 4
#define GNS_FLAGS_ALL (1 << GNS_GNGGA | 1 << GNS_GNGSA | 1 << GNS_GNGSV | 1 << GNS_GNRMC)

#define GNS_CONCAT(x, y) (uint16_t)((x) << 8 | (y))


static uint8_t BufferData[GNS_BUFFER_SIZE];
static BUFFER_t BUFFER;
static GNS_Int_t Int;
static uint8_t ReceivedFlags = 0x00;


/** 
 * @brief  字符串转INT
 * @note   
 * @param  *ptr: 字符串
 * @param  *cnt: 长度
 * @retval 
 */
static int32_t ParseIntNumber(const char *ptr, uint8_t *cnt)
{
	uint8_t minus = 0, i = 0;
	int32_t sum = 0;

	if (*ptr == '-')
	{
		minus = 1;
		ptr++;
		i++;
	}
	while (CHARISNUM(*ptr))
	{
		sum = 10 * sum + CHARTONUM(*ptr);
		ptr++;
		i++;
	}
	if (cnt != NULL)
	{ 
		*cnt = i;
	}
	if (minus)
	{ 
		return -sum;
	}
	return sum;
}

/** 
 * @brief  16进制字符串转int
 * @note   
 * @param  *ptr: 
 * @param  *cnt: 
 * @retval 
 */
static uint32_t ParseHexNumber(const char *ptr, uint8_t *cnt)
{
	uint8_t i = 0;
	uint32_t sum = 0;

	while (CHARISHEXNUM(*ptr))
	{ 
		sum = 16 * sum + CHARHEXTONUM(*ptr);
		ptr++;
		i++;
	}
	if (cnt != NULL)
	{
		*cnt = i;
	}
	return sum; 
}

/** 
 * @brief  字符串转float
 * @note   
 * @param  *ptr: 
 * @param  *cnt: 
 * @retval 
 */
static float ParseFloatNumber(const char *ptr, uint8_t *cnt)
{
	uint8_t i = 0, j = 0;
	float sum = 0.0f;

	sum = (float)ParseIntNumber(ptr, &i); 
	j += i;
	ptr += i;
	if (*ptr == '.')
	{ 
		float dec;
		dec = (float)ParseIntNumber(ptr + 1, &i) / (float)pow(10, i);
		if (sum >= 0)
		{
			sum += dec;
		}
		else
		{
			sum -= dec;
		}
		j += i + 1;
	}

	if (cnt != NULL)
	{ 
		*cnt = j;
	}
	return sum; 
}

/** 
 * @brief  解析经纬度
 * @note   
 * @param  *term: 
 * @retval 
 */
static float ParseLatLong(const char *term)
{
	float num;
	float tmp;
	uint8_t cnt;

	if (term[4] == '.')
	{
		tmp = (float)ParseIntNumber(&term[5], &cnt);
		tmp /= (60.0f * (float)pow(10, cnt)); /* Parse seconds */

		num = (float)(10 * CHARTONUM(term[0]) + CHARTONUM(term[1]));		  /* Parse degrees */
		num += (float)(10 * CHARTONUM(term[2]) + CHARTONUM(term[3])) / 60.0f; /* Parse minutes */
		num += tmp;
	}
	else
	{
		tmp = (float)ParseIntNumber(&term[6], &cnt);
		tmp /= (60.0f * (float)pow(10, cnt)); /* Parse seconds */

		num = (float)(100 * CHARTONUM(term[0]) + 10 * CHARTONUM(term[1]) + CHARTONUM(term[2])); /* Parse degrees */
		num += (float)(10 * CHARTONUM(term[3]) + CHARTONUM(term[4])) / 60.0f;					/* Parse minutes */
		num += tmp;
	}
	return num;
}

/** 
 * @brief  匹配语句头
 * @note   
 * @param  *GNS: 
 * @retval None
 */
static void ParseValue(GNS_t *GNS)
{
	uint8_t i;
	if (Int.Flags.F.Term_Num == 0)
	{ 
		if (strcmp(Int.Term, FROMMEM("$GNGGA")) == 0)
		{
			Int.Flags.F.Statement = GNS_GNGGA;
		}
		else if (strcmp(Int.Term, FROMMEM("$GNGSA")) == 0)
		{
			Int.Flags.F.Statement = GNS_GNGSA;
		}
		else if (strcmp(Int.Term, FROMMEM("$GNGSV")) == 0)
		{
			Int.Flags.F.Statement = GNS_GNGSV;
		}
		else if (strcmp(Int.Term, FROMMEM("$GNRMC")) == 0)
		{
			Int.Flags.F.Statement = GNS_GNRMC;
		}
		else
		{
			Int.Flags.F.Statement = GNS_UNKNOWN;
		}
		strcpy(Int.Statement, Int.Term); 
		return;							
	}


	for (i = 0; i < GNS->CustomStatementsCount; i++)
	{ 
		if (Int.Flags.F.Term_Num == GNS->CustomStatements[i]->TermNumber && strcmp(GNS->CustomStatements[i]->Statement, Int.Statement) == 0)
		{
			switch (GNS->CustomStatements[i]->Type)
			{
			case GNS_CustomType_String: 
				strcpy(GNS->CustomStatements[i]->Value.S, Int.Term);
				break;
			case GNS_CustomType_Char:
				GNS->CustomStatements[i]->Value.C = Int.Term[0];
				break;
			case GNS_CustomType_Int:
				GNS->CustomStatements[i]->Value.I = ParseIntNumber(Int.Term, NULL);
				break;
			case GNS_CustomType_Float:
				GNS->CustomStatements[i]->Value.F = ParseFloatNumber(Int.Term, NULL);
				break;
			case GNS_CustomType_LatLong: 
				GNS->CustomStatements[i]->Value.L = ParseLatLong(Int.Term);
				break;
			default:
				break;
			}
			GNS->CustomStatements[i]->Updated = 1; 
		}
	}

	switch (GNS_CONCAT(Int.Flags.F.Statement, Int.Flags.F.Term_Num))
	{
	/* GNGGA */
	case GNS_CONCAT(GNS_GNGGA, 1)://UTC时间
		GNS->Time.Hours = 10 * CHARTONUM(Int.Term[0]) + CHARTONUM(Int.Term[1]);
		GNS->Time.Minutes = 10 * CHARTONUM(Int.Term[2]) + CHARTONUM(Int.Term[3]);
		GNS->Time.Seconds = 10 * CHARTONUM(Int.Term[4]) + CHARTONUM(Int.Term[5]);
		if (Int.Term[6] == '.')
		{
			uint8_t cnt;
			uint16_t tmp = ParseIntNumber(&Int.Term[7], &cnt);

			switch (cnt)
			{
			case 1:
				GNS->Time.Hundreds = 10 * tmp;
				GNS->Time.Thousands = 100 * tmp;
				break;
			case 2:
				GNS->Time.Hundreds = (uint8_t)tmp;
				GNS->Time.Thousands = 10 * tmp;
				break;
			case 3:
				GNS->Time.Hundreds = tmp / 10;
				GNS->Time.Thousands = tmp;
				break;
			}
		}
		break;
	case GNS_CONCAT(GNS_GNGGA, 2):				
		GNS->Latitude = ParseLatLong(Int.Term); 
		break;
	case GNS_CONCAT(GNS_GNGGA, 3): 
		if (Int.Term[0] == 'S' || Int.Term[0] == 's')
		{
			GNS->Latitude = -GNS->Latitude;
		}
		break;
	case GNS_CONCAT(GNS_GNGGA, 4):				
		GNS->Longitude = ParseLatLong(Int.Term);
		break;
	case GNS_CONCAT(GNS_GNGGA, 5): 
		if (Int.Term[0] == 'W' || Int.Term[0] == 'w')
		{
			GNS->Longitude = -GNS->Longitude;
		}
		break;
	case GNS_CONCAT(GNS_GNGGA, 6):					
		GNS->Fix = (GNS_Fix_t)CHARTONUM(Int.Term[0]);
		break;
	case GNS_CONCAT(GNS_GNGGA, 7): 
		GNS->SatsInUse = ParseIntNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNGGA, 9): 
		GNS->Altitude = ParseFloatNumber(Int.Term, NULL);
		break;

	/* GNGSA */
	case GNS_CONCAT(GNS_GNGSA, 2): 
		GNS->FixMode = (GNS_FixMode_t)ParseIntNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNGSA, 3):
	case GNS_CONCAT(GNS_GNGSA, 4):
	case GNS_CONCAT(GNS_GNGSA, 5):
	case GNS_CONCAT(GNS_GNGSA, 6):
	case GNS_CONCAT(GNS_GNGSA, 7):
	case GNS_CONCAT(GNS_GNGSA, 8):
	case GNS_CONCAT(GNS_GNGSA, 9):
	case GNS_CONCAT(GNS_GNGSA, 10):
	case GNS_CONCAT(GNS_GNGSA, 11):
	case GNS_CONCAT(GNS_GNGSA, 12):
	case GNS_CONCAT(GNS_GNGSA, 13):
	case GNS_CONCAT(GNS_GNGSA, 14):
		GNS->SatelliteIDs[Int.Flags.F.Term_Num - 3] = ParseIntNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNGSA, 15): 
		GNS->PDOP = ParseFloatNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNGSA, 16): 
		GNS->HDOP = ParseFloatNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNGSA, 17):
		GNS->VDOP = ParseFloatNumber(Int.Term, NULL);
		break;

	/* GNRMC  */
	case GNS_CONCAT(GNS_GNRMC, 2): 
		GNS->Valid = Int.Term[0] == 'A';
		break;
	case GNS_CONCAT(GNS_GNRMC, 7): 
		GNS->Speed = ParseFloatNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNRMC, 8): 
		GNS->Coarse = ParseFloatNumber(Int.Term, NULL);
		break;
	case GNS_CONCAT(GNS_GNRMC, 9): 
		GNS->Date.Day = 10 * CHARTONUM(Int.Term[0]) + CHARTONUM(Int.Term[1]);
		GNS->Date.Month = 10 * CHARTONUM(Int.Term[2]) + CHARTONUM(Int.Term[3]);
		GNS->Date.Year = 2000 + 10 * CHARTONUM(Int.Term[4]) + CHARTONUM(Int.Term[5]);
		break;
	case GNS_CONCAT(GNS_GNRMC, 10): 
		GNS->Variation = ParseFloatNumber(Int.Term, NULL);
		break;

	/* GNGSV */
	case GNS_CONCAT(GNS_GNGSV, 1): 
		Int.Flags.F.GNGSV_Nums = CHARTONUM(Int.Term[0]);
		break;
	case GNS_CONCAT(GNS_GNGSV, 2):
		Int.Flags.F.GNGSV_Num = CHARTONUM(Int.Term[0]);
		break;
	case GNS_CONCAT(GNS_GNGSV, 3):
		GNS->SatsInView = ParseIntNumber(Int.Term, NULL);
		break;
	default:
		if (Int.Flags.F.Statement == GNS_GNGSV && Int.Flags.F.Term_Num >= 4)
		{ 
			uint32_t tmp;
			uint8_t mod, term_num;

			tmp = ParseIntNumber(Int.Term, NULL);   
			term_num = Int.Flags.F.Term_Num - 4; 

			mod = term_num % 4;											
			term_num = (Int.Flags.F.GNGSV_Num - 1) * 4 + (term_num / 4); 

			if (term_num < GNS_MAX_SATS_IN_VIEW)
			{ 
				switch (mod)
				{
				case 0:
					GNS->SatsDesc[term_num].ID = tmp;
					break;
				case 1:
					GNS->SatsDesc[term_num].Elevation = tmp;
					break;
				case 2: 
					GNS->SatsDesc[term_num].Azimuth = tmp;
					break;
				case 3: 
					GNS->SatsDesc[term_num].SNR = tmp;
					break;
				default:
					break;
				}
			}
		}
		break;
	}
}
/** 
 * @brief  初始化内存空间
 * @note   
 * @param  *GNS: 
 * @retval 
 */
GNS_Result_t GNS_Init(GNS_t *GNS)
{
	memset((void *)GNS, 0x00, sizeof(GNS_t));			  /* Reset structure for GNS */
	BUFFER_Init(&BUFFER, sizeof(BufferData), BufferData); /* Initialize buffer for received data */

	return gpsOK;
}
/** 
 * @brief  获取到新数据增加到缓存
 * @note   
 * @param  *ch: 缓存
 * @param  count: 长度
 * @retval 
 */
uint32_t GNS_DataReceived(uint8_t *ch, size_t count)
{
	return BUFFER_Write(&BUFFER, ch, count); 
}
/** 
 * @brief  解析数据
 * @note   
 * @param  *GNS: 
 * @retval 
 */
GNS_Result_t GNS_Update(GNS_t *GNS)
{
	uint8_t ch;
	static uint8_t waitingFirst = 1;
	while (BUFFER_Read(&BUFFER, &ch, 1))
	{
		if (ch == '$')
		{												 
			memset((void *)&Int, 0x00, sizeof(GNS_Int_t)); 
			Int.CRC = 0x00;
			GNS_ADDTOTERM(ch); 
		}
		else if (ch == ',')
		{
			GNS_ADDTOCRC(ch);	 
			ParseValue(GNS);	 
			GNS_START_NEXT_TERM(); 
		}
		else if (ch == '*')
		{
			Int.Flags.F.Star = 1;  
			ParseValue(GNS);	   
			GNS_START_NEXT_TERM(); 
		}
		else if (ch == '\r')
		{
			if ((uint8_t)ParseHexNumber(Int.Term, NULL) == Int.CRC)
			{ /* CRC is OK data valid */
				switch (Int.Flags.F.Statement)
				{
				case GNS_GNGGA:
				case GNS_GNGSA:
				case GNS_GNRMC:
					ReceivedFlags |= 1 << Int.Flags.F.Statement;
					break;
				case GNS_GNGSV:
					if (Int.Flags.F.GNGSV_Num == Int.Flags.F.GNGSV_Nums)
					{
						ReceivedFlags |= 1 << Int.Flags.F.Statement;
					}
				default:
					break;
				}
			}
		}
		else if (ch != ' ')
		{ 
			if (!Int.Flags.F.Star)//没有见到到*号
			{					 
				GNS_ADDTOCRC(ch);
			}
			GNS_ADDTOTERM(ch); 
		}
		if ((ReceivedFlags & GNS_FLAGS_ALL) == GNS_FLAGS_ALL)
		{
			uint8_t ok = 1, i;
			for (i = 0; i < GNS->CustomStatementsCount; i++)
			{
				if (!GNS->CustomStatements[i]->Updated)
				{
					ok = 0;
					break;
				}
			}

			if (ok)
			{
				ReceivedFlags = 0x00; 
				for (i = 0; i < GNS->CustomStatementsCount; i++)
				{ 
					GNS->CustomStatements[i]->Updated = 0;
				}
				waitingFirst = 0;  
				return gpsNEWDATA; 
			}
		}
	}
	if (waitingFirst)
	{					 
		return gpsNODATA; 
	}
	return gpsOLDDATA;
}
/** 
 * @brief  添加自定义解析头 
 * @note   
 * @param  *GNS: 结构体指针
 * @param  *Custom: 结构体指针
 * @param  *GPG_Statement: NMEA
 * @param  TermNumber: 位置
 * @param  Type: 类型
 * @retval 
 */
GNS_Result_t GNS_Custom_Add(GNS_t *GNS, GNS_Custom_t *Custom, const char *GPG_Statement, uint8_t TermNumber, GNS_CustomType_t Type)
{
	if (GNS->CustomStatementsCount >= GNS_CUSTOM_COUNT)
	{
		return gpsERROR;
	}

	Custom->Statement = GPG_Statement; 
	Custom->TermNumber = TermNumber;   
	Custom->Type = Type;			 
	Custom->Updated = 0;			 

	GNS->CustomStatements[GNS->CustomStatementsCount++] = Custom; 
	return gpsOK;												  
}
/** 
 * @brief  删除自定义语句
 * @note   
 * @param  *GNS: 
 * @param  *Custom: 
 * @retval 
 */
GNS_Result_t GNS_Custom_Delete(GNS_t *GNS, GNS_Custom_t *Custom)
{
	uint16_t i;
	if (GNS->CustomStatementsCount == 0)
	{
		return gpsERROR;
	}


	for (i = 0; i < GNS->CustomStatementsCount; i++)
	{ 
		if (GNS->CustomStatements[i] == Custom)
		{
			break;
		}
	}


	if (i < GNS->CustomStatementsCount)
	{ 
		for (; i < GNS->CustomStatementsCount - 1; i++)
		{
			GNS->CustomStatements[i] = GNS->CustomStatements[i + 1];
		}
		GNS->CustomStatements[i] = 0; 
		GNS->CustomStatementsCount--;
		return gpsOK;				  
	}
	return gpsERROR; 
}
/** 
 * @brief  计算两个坐标的距离
 * @note   
 * @param  *Distance: 
 * @retval 
 */
GNS_Result_t GNS_DistanceBetween(GNS_Distance_t *Distance)
{
	float f1, f2, l1, l2, df, dfi, a;
	f1 = GNS_DEGREES2RADIANS(Distance->LatitudeStart);
	f2 = GNS_DEGREES2RADIANS(Distance->LatitudeEnd);
	l1 = GNS_DEGREES2RADIANS(Distance->LongitudeStart);
	l2 = GNS_DEGREES2RADIANS(Distance->LongitudeEnd);
	df = GNS_DEGREES2RADIANS(Distance->LatitudeEnd - Distance->LatitudeStart);
	dfi = GNS_DEGREES2RADIANS(Distance->LongitudeEnd - Distance->LongitudeStart);

	a = (float)(sin(df * 0.5f) * sin(df * 0.5f) + cos(f1) * cos(f2) * sin(dfi * 0.5f) * sin(dfi * 0.5f));
	Distance->Distance = (float)(GNS_EARTH_RADIUS * 2.0f * atan2(sqrt(a), sqrt(1 - a)) * 1000.0f); /* Get distance in meters */

	df = (float)(sin(l2 - l1) * cos(f2));
	dfi = (float)(cos(f1) * sin(f2) - sin(f1) * cos(f2) * cos(l2 - l1));
	Distance->Bearing = (float)(GNS_RADIANS2DEGREES(atan2(df, dfi)));

	if (Distance->Bearing < 0)
	{
		Distance->Bearing += 360;
	}
	return gpsOK;
}
/** 
 * @brief  对速度进行单位变换
 * @note   
 * @param  SpeedInKnots: 
 * @param  toSpeed: 
 * @retval 
 */
float GNS_ConvertSpeed(float SpeedInKnots, GNS_Speed_t toSpeed)
{
	switch (toSpeed)
	{
	/* 公制 */
	case GNS_Speed_KilometerPerSecond:
		return SpeedInKnots * 0.000514f;
	case GNS_Speed_MeterPerSecond:
		return SpeedInKnots * 0.5144f;
	case GNS_Speed_KilometerPerHour:
		return SpeedInKnots * 1.852f;
	case GNS_Speed_MeterPerMinute:
		return SpeedInKnots * 30.87f;

	/* 英制 */
	case GNS_Speed_MilePerSecond:
		return SpeedInKnots * 0.0003197f;
	case GNS_Speed_MilePerHour:
		return SpeedInKnots * 1.151f;
	case GNS_Speed_FootPerSecond:
		return SpeedInKnots * 1.688f;
	case GNS_Speed_FootPerMinute:
		return SpeedInKnots * 101.3f;

	/*转换为步--不准确*/
	case GNS_Speed_MinutePerKilometer:
		return SpeedInKnots * 32.4f;
	case GNS_Speed_SecondPerKilometer:
		return SpeedInKnots * 1944.0f;
	case GNS_Speed_SecondPer100Meters:
		return SpeedInKnots * 194.4f;
	case GNS_Speed_MinutePerMile:
		return SpeedInKnots * 52.14f;
	case GNS_Speed_SecondPerMile:
		return SpeedInKnots * 3128.0f;
	case GNS_Speed_SecondPer100Yards:
		return SpeedInKnots * 177.7f;

	/* 海里 */
	case GNS_Speed_SeaMilePerHour:
		return SpeedInKnots * 1.0f;
	default:
		return 0;
	}
}
